/*************************************************************/
/**            USEFUL MATHEMATICAL FUNCTIONS                **/
/**  ©Copyright 2019 Alexandre GAILLARD, Sumudu KANKANAMGE  **/
/*************************************************************/


/** MATHEMATICAL FUNCTIONS **/
#define max(a,b) (((a)>(b))?(a):(b))
#define min(a,b) (((a)<(b))?(a):(b))
#define interpol(x,y,z) (y+(x-floor(x))*(z-y))
#define inter1d(x1,y1,y2) ((1.0-(x1))*(y1)+(x1)*(y2))
#define inter2d(x1,x2,y11,y21,y12,y22) ((1.0-(x2))*((1.0-(x1))*(y11)+(x1)*(y21))+(x2)*((1.0-(x1))*(y12)+(x1)*(y22)))


/** GRID SPACING **/
#define linspace(x0,xmax,n,i) ((i)*(((xmax)-(x0))/(n-1))+(x0)) // transform a grid into a value
#define invlinspace(x0,xmax,n,x) ((((x)-(x0))/((xmax)-(x0)))*(n-1)) // transform a value into a grid
#define expspace(i,xmin,xmax,echelle,n) ( echelle*(exp((log((xmax/echelle)-((xmin/echelle)-1.0))/(n-1))*(i))+((xmin/echelle)-1.0)) ) // transform a grid into a value
#define invexpspace(x,xmin,xmax,echelle,n) ( log((x)/echelle - ((xmin/echelle)-1.0))/(log((xmax/echelle)-((xmin/echelle)-1.0))/(n-1)) ) // transform a value into a grid
// this spacing mix an exponential and then an equispaced grid //
#define fungrid(i,xmin1,xmax1,xmin2,xmax2,echelle,n1,n2) ((i)<(n1))?(echelle*(exp((log((xmax1/echelle)-((xmin1/echelle)-1.0))/(n1-1))*(i))+((xmin1/echelle)-1.0))):((i-n1+1)*(((xmax2)-(xmin2))/(n2))+(xmin2))
#define invfungrid(x,xmin1,xmax1,xmin2,xmax2,echelle,n1,n2) ((x)<=(xmax1))?(log((x)/echelle - ((xmin1/echelle)-1.0))/(log((xmax1/echelle)-((xmin1/echelle)-1.0))/(n1-1))):((((x)-(xmin2))/((xmax2)-(xmin2)))*(n2)+n1-1)

/** COMPUTE DERIVATIVE **/
#define deriv(val1,val2,val3,x1,x2,x3) ((1.0 - (x3 - x2)/(x3 - x1))*((val3 - val2)/(x3-x2)) + ((x3 - x2)/(x3 - x1))*((val2 - val1)/(x2-x1)))



/** SIGN FUNCTION **/
//template<class T>
//inline const T SIGN(const T &a, const T &b)
//    {return b >= 0 ? (a >= 0 ? a : -a) : (a >= 0 ? -a : a);}
//
//inline float SIGN(const float &a, const double &b)
//    {return b >= 0 ? (a >= 0 ? a : -a) : (a >= 0 ? -a : a);}
//
//inline float SIGN(const double &a, const float &b)
//    {return b >= 0 ? (a >= 0 ? a : -a) : (a >= 0 ? -a : a);}


/** TIMER FUNCTION **/
double timer_fun(timeval t1, timeval t2){
    double elapsedTime;
    elapsedTime = (t2.tv_sec - t1.tv_sec) * 1000.0;
    elapsedTime += (t2.tv_usec - t1.tv_usec) / 1000.0;
    
    return (elapsedTime/1000);
}


/** QUADRATIC INTERPOLATION **/
double interQuad1d(const double dx, const double h0, const double h1, const double h2) // dx lies in the interval [i-0.5; i+0.5] (distance is equal to 1), we fake f(i-0.5);
{
double h05, h15, a, b, c, fapprox;

h05 = (h1+h0)/2.0;
h15 = (h2+h1)/2.0;

c = h05;
b = 2.0*(h1-h05);
a = h15-2.0*h1+h05;

fapprox = a*dx*dx+b*dx+c;

return fapprox;

}



/** TRILINEAR INTERPOLATION **/
double inter3d(const double dx, const double dy, const double dz, const double c000, const double c001, const double c010, const double c011, const double c100, const double c101, const double c110, const double c111)
{
    double c00, c01, c10, c11, c0, c1, c;
    
    c00 = c000*(1.0-dx)+c100*dx;
    c01 = c001*(1.0-dx)+c101*dx;
    c10 = c010*(1.0-dx)+c110*dx;
    c11 = c011*(1.0-dx)+c111*dx;
    
    c0 = c00*(1.0-dy)+c10*dy;
    c1 = c01*(1.0-dy)+c11*dy;
    
    c = c0*(1.0-dz)+c1*dz;
    
    return c;
}




/**  FIND THE WEIGHT FOR INTERPOLATION **/
double weightinter(double x, double xgrid, int *ixgrid, double vector[], int dimweight)
{
    double dxgrid;
    
    // PUT THESE VALUE ON A GRID //
    *ixgrid=min((dimweight-1),(int)(floor(xgrid)));
    *ixgrid=max(0,*ixgrid);
    
    if (*ixgrid>=(dimweight-1))
    {
        xgrid=(dimweight-1)-0.000000001;
        *ixgrid=min((dimweight-1),(int)(floor(xgrid)));
        *ixgrid=max(0,*ixgrid);
    }
    
    if (*ixgrid<=0)
    {
        xgrid=0.000000001;
        *ixgrid=min((dimweight-1),(int)(floor(xgrid)));
        *ixgrid=max(0,*ixgrid);
    }
    
    dxgrid=(x-vector[*ixgrid])/(vector[(*ixgrid+1)]-vector[*ixgrid]);
    
    return dxgrid;
    
}




/** COMPARE VECTORS **/
int comparefun2(const void* a, const void* b) 
{ 
	double* da = (double*)a; 
	double* db = (double*)b; 
	int diff1 = (da[0] > db[0]) - (da[0] < db[0]); 
	if (diff1 != 0) return diff1; 
	return (da[1] > db[1]) - (da[1] < db[1]); 
}




/** SHIFTER **/
void shft2(double &a, double &b, const double c){a=b;b=c;}
void shft3(double &a, double &b, double &c, const double d){a=b;b=c;c=d;}



/** BASCULE FUNCTIONS **/
void bascule(double *VectorIN, double *VectorOUT, int dim)
{
    int i;
    for(i=0; i<dim; i++){VectorOUT[i]=VectorIN[i];}
}

void bascule_zero(double *VectorIN, double *VectorOUT, int dim)
{
    int i;
    for(i=0; i<dim; i++){
        VectorOUT[i]=VectorIN[i];
        VectorIN[i] = 0.0;
    }
}

void vector_zero(double *VectorIN,int dim)
{
    for(int i=0; i<dim; i++){
        VectorIN[i] = 0.0;
    }
}



/** GET RESIDUALS OF THE HISTOGRAM **/
void weighthist(double x, double xgrid, double *residout, int *ixgrid, double vector[], int dimweight)
{
    
    // PUT THESE VALUE ON A GRID //
    *ixgrid=min((dimweight-1),(int)(floor(xgrid)));
    *ixgrid=max(0,*ixgrid);
    
    if (*ixgrid>=(dimweight-1))
    {
        printf("getresid: (ixgrid>(dimweight-1))");getchar();
        *ixgrid=min((dimweight-1),(int)(floor(xgrid)));
        *ixgrid=max(0,*ixgrid);
    }
    
    if (*ixgrid<=0)
    {
        printf("getresid: (ixgrid<0)");getchar();
        *ixgrid=min((dimweight-1),(int)(floor(xgrid)));
        *ixgrid=max(0,*ixgrid);
    }
    
    *residout=(x-vector[*ixgrid])/(vector[(*ixgrid+1)]-vector[*ixgrid]);
}






// function to get median.
double quantileworth(double *distr,const double fulldist, double WEALTH[], int dim)
{
    int i;
    double dfrac,dfracold,prop,medianworthout;
    
    if (fulldist>0.0)
    {
    i=0;
    dfrac=0.0;
    dfrac+=distr[i]/fulldist;
    
    if (dfrac>=0.33)
    {
        medianworthout=WEALTH[i];
    }
    else
    {
        while ((dfrac<0.33) && (i<(dim-1)))
        {
            dfracold=dfrac;
            i++;
            dfrac+=distr[i]/fulldist;
        }
        prop=(0.33-dfracold)/(dfrac-dfracold);
        medianworthout=(1.0-prop)*WEALTH[(i-1)]+prop*WEALTH[i];
    }
    }
    else
    {
        medianworthout=0.0;
    }
    
    return(medianworthout);
}


// function to get median.
void medianworth(double *distr,const double fulldist, int dim, double *medianworthout)
{
    int i;
    double dfrac,dfracold,prop;
    
    if (fulldist>0.0)
    {
    i=0;
    dfrac=0.0;
    dfrac+=distr[i]/fulldist;
    
    if (dfrac>=0.5)
    {
        *medianworthout=grid[i];
    }
    else
    {
        while ((dfrac<0.5) && (i<(dim-1)))
        {
            dfracold=dfrac;
            i++;
            dfrac+=distr[i]/fulldist;
        }
        prop=(0.5-dfracold)/(dfrac-dfracold);
        *medianworthout=(1.0-prop)*grid[(i-1)]+prop*grid[i];
    }
    }
    else
    {
        *medianworthout=0.0;
    }
    
}




/** INVARIANT DISTRIBUTION **/

/** INVARIANT DISTRIBUTION **/
template<size_t dim>
void inv_distri(double (&invdist)[dim], double (&prob)[dim][dim])
{
    double tempdist[dim], critdist, sumdist;
    int i, j;
    
    invdist[1] = 1.0;
    
    critdist   = 1.0;
    while(critdist > 0.00000001) {
        
        for(i=0;i<dim;i++){tempdist[i] = invdist[i];}
        
        // compute the invdist //
        for(i = 0; i<dim; i++){invdist[i] = 0.0;}
        
        for(i = 0; i<dim; i++){for(j = 0; j<dim; j++){invdist[i] += tempdist[j]*prob[j][i];}}
        
        critdist = 0.0;
        for(i=0;i<dim;i++){critdist = max(fabs(invdist[i] - tempdist[i]), critdist);}
        
//       printf("%f %f %f %f %f %f %f", invdist[0], invdist[1], invdist[2], tempdist[0], tempdist[1], tempdist[2], critdist); getchar();
        
    }
    
    // renormalize invdist //
    sumdist = 0.0;
    for(i = 0; i<dim; i++) {sumdist += invdist[i];}
    for(i=0;i<dim;i++) {invdist[i] = invdist[i]/sumdist;}
    
}



//sumudu: just in case a new invariant function with pointers and explicit dimension for g++
//void inv_distriX(double *invdist, double **prob, int dim, int dim2)
//{
//    double tempdist[dim], critdist, sumdist;
//    int i, j;
//
//    invdist[1] = 1.0;
//
//    critdist = 1.0;
//    while(critdist > 0.00000001) {
//
//        for(i=0;i<dim;i++){
//            tempdist[i] = invdist[i];
//        }
//
//        // compute the invdist //
//        for(i = 0; i<dim; i++){
//            invdist[i] = 0.0;
//        }
//
//        for(i = 0; i<dim; i++){
//            for(j = 0; j<dim2; j++) {
//                invdist[i] += tempdist[j]*prob[j][i];
//            }
//        }
//
//        critdist = 0.0;
//        for(i=0;i<dim;i++) {
//            critdist = max(abs(invdist[i] - tempdist[i]), critdist);
//        }
//
//        //       printf("%f %f %f %f %f %f %20.15f\n", invdist[0], invdist[1], invdist[2], tempdist[0], tempdist[1], tempdist[2], critdist); //getchar();
//
//    }
//
//    // renormalize invdist //
//    sumdist = 0.0;
//    for(i = 0; i<dim; i++) {
//        sumdist += invdist[i];
//    }
//    for(i=0;i<dim;i++) {
//        invdist[i] = invdist[i]/sumdist;
//    }
//
//}






double GINI(double *var, double *mass, int size) {
    int j,i;
    double *aera, *varcumul, *masscumul, *varcumulw, totalwealth, gini;
    
    aera = (double *) calloc((size+1), sizeof(double));
    varcumul = (double *) calloc((size+1), sizeof(double));
    masscumul = (double *) calloc((size+1), sizeof(double));
    varcumulw = (double *) calloc((size+1), sizeof(double));
    
    totalwealth = 0.0;
    masscumul[0] = 0.0;
    varcumul[0] = 0.0;
    varcumulw[0] = 0.0;
    
    for(j=0; j < size; j++) {
        totalwealth += var[j] * mass[j];
    }
    
    for(j=1; j < (size+1); j++) {
        masscumul[j] = masscumul[j-1] + mass[j-1];
        varcumul[j] = varcumul[j-1] + var[j-1]*mass[j-1];
        varcumulw[j] = varcumul[j]/totalwealth;
    }
    
    aera[0] = 0;
    for(i=1; i < (size+1); i++) {
        aera[i] = aera[i-1] + (varcumulw[i] + varcumulw[i-1])*(masscumul[i] - masscumul[i-1]);
    }
    
    gini = 1 - aera[size];
    
    free(aera);
    free(varcumul);
    free(varcumulw);
    free(masscumul);
    
    return gini;
}




double GINI_sort(double vector[][2], int size) {
    int j,i;
    double *aera, *varcumul, *masscumul, *varcumulw, totalwealth, gini;
    
    aera        = (double *) calloc((size+1), sizeof(double));
    varcumul    = (double *) calloc((size+1), sizeof(double));
    masscumul   = (double *) calloc((size+1), sizeof(double));
    varcumulw   = (double *) calloc((size+1), sizeof(double));
    
    totalwealth     = 0.0;
    masscumul[0]    = 0.0;
    varcumul[0]     = 0.0;
    varcumulw[0]    = 0.0;
    
    for(j=0; j < size; j++) {
        totalwealth += vector[j][0] * vector[j][1];
    }
    
    for(j=1; j < (size+1); j++) {
        masscumul[j] = masscumul[j-1] + vector[j-1][1];
        varcumul[j] = varcumul[j-1] + vector[j-1][0]*vector[j-1][1];
        varcumulw[j] = varcumul[j]/totalwealth;
    }
    
    aera[0] = 0;
    for(i=1; i < (size+1); i++) {
        aera[i] = aera[i-1] + (varcumulw[i] + varcumulw[i-1])*(masscumul[i] - masscumul[i-1]);
    }
    
    gini = 1 - aera[size];
    
    free(aera);
    free(varcumul);
    free(varcumulw);
    free(masscumul);
    
    return gini;
}





// Function to get top X% of the population detening Y% of the wealth
double top_threshold_fun(double vector[][2], int size, double percent) {
    int iter;
    double massCUM,dprop;

    massCUM         = 0.0;
    iter            = 0;
    
    while(massCUM < percent){massCUM += vector[iter][1];iter++;}
    dprop   = (massCUM - vector[iter-1][1]);
    //printf("%f,%f,%f,%f",percent,massCUM,dprop,inter1d(dprop,vector[iter-1][0],vector[iter][0]));getchar();
    
    return (inter1d(dprop,vector[iter-1][0],vector[iter][0]));
}



#define interPOL(x1,y1,y2) ((1.0-(x1))*(y1)+(x1)*(y2))
#define interPOL(x1,y1,y2) ((1.0-(x1))*(y1)+(x1)*(y2))

// Function to get top X% of the population detening Y% of the wealth
double toppercent(double *var, double *mass, int size, const double percent) {
    int j,i,iter;
    double *varcumul, *varcumulw, totalwealth, massCUM, topXval, dprop;
    
    
//    printf("%f, %f, %f", percent, mass[4], var[4]); getchar();
    varcumul = (double *) calloc((size+1), sizeof(double));
    varcumulw = (double *) calloc((size+1), sizeof(double));
    
    totalwealth = 0.0;
    varcumul[0] = 0.0;
    varcumulw[0] = 0.0;
    
    for(j=0; j < size; j++) {
        totalwealth += var[j] * mass[j];
    }
    
    for(j=1; j < (size+1); j++) {
        varcumul[j] = varcumul[j-1] + var[j-1]*mass[j-1];
        varcumulw[j] = varcumul[j]/totalwealth;
    }

    massCUM = 0.0;
    iter = 0.0;
    while(massCUM < percent) {
        massCUM += mass[iter];
        iter++;
    }
    dprop = massCUM - percent;
    // Apply an 1D interpolation to get closer value to the truth.
    topXval = inter1d(dprop, varcumulw[iter - 1], varcumulw[iter]);
//    printf("%f, %f, %f, %f, %d, %d, %f, %f, %f, %f",*topXval, dprop, inter1d(dprop, varcumulw[iter - 1], varcumulw[iter]), percent, iter, size, mass[4], var[4], varcumul[4], varcumulw[4]);getchar();
    // without inter1D:
    // topXval = varcumulw[iter - 1];
    
    free(varcumulw);
    free(varcumul);
    
    return topXval;
}

